#include "Matrix.h"
#include "MathDefine.h"

#include <math.h>

const Matrix Matrix::IDENTITY( 1.f, 0.f, 0.f, 0.f,
							   0.f, 1.f, 0.f, 0.f,
							   0.f, 0.f, 1.f, 0.f,
							   0.f, 0.f, 0.f, 1.f );
Matrix::Matrix(void) :
D3DXMATRIX( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 )
{
}

Matrix::Matrix(float m11, float m12, float m13, float m14,
		float m21, float m22, float m23, float m24,
		float m31, float m32, float m33, float m34,
		float m41, float m42, float m43, float m44 ) :
D3DXMATRIX( m11, m12, m13, m14,
		   m21, m22, m23, m24,
		   m31, m32, m33, m34,
		   m41, m42, m43, m44 )
{
}

Matrix::Matrix( const D3DXMATRIX & matrix ) :
D3DXMATRIX( matrix )
{
}

Matrix::Matrix( const Quaternion & quaternion )
{
	MakeRotation( quaternion );
}

Matrix::Matrix( const Vector3 & position, const Quaternion & quaternion )
{
	MakeRotation( quaternion );
	SetTranslation( position );
}

Matrix::Matrix( const Vector3 & position, const Vector3 & ypr )
{
	MakeRotation( ypr );
	SetTranslation( position );
}

Matrix::~Matrix(void)
{
}

void Matrix::MakeTransform( const Vector3 & pos, const Vector3 & ypr )
{
	MakeRotation( ypr );
	SetTranslation( pos );
}

void Matrix::MakeScaling( const float x, const float y, const float z )
{
	D3DXMatrixScaling( this, x, y, z );
}

void Matrix::MakeTranslation( const float x, const float y, const float z )
{
	D3DXMatrixTranslation( this, x, y, z );
}

void Matrix::MakeTranslation( const Vector3 & pos )
{
	D3DXMatrixTranslation( this, pos.x, pos.y, pos.z );
}

void Matrix::MakeRotationX( const float radian )
{
	D3DXMatrixRotationX( this, radian );
}

void Matrix::MakeRotationY( const float radian )
{
	D3DXMatrixRotationY( this, radian );
}

void Matrix::MakeRotationZ( const float radian )
{
	D3DXMatrixRotationZ( this, radian );
}

void Matrix::MakeRotation( const float y, const float p, const float r )
{
	D3DXMatrixRotationYawPitchRoll( this, y, p, r );
}

void Matrix::MakeRotation( const Vector3 & ypr )
{
	D3DXMatrixRotationYawPitchRoll( this, ypr.x, ypr.y, ypr.z );
}

void Matrix::MakeRotation( const Quaternion & quat )
{
}

void Matrix::MakeRotationFromDirection( const Vector3 & direction, const Vector3 & up )
{
	Vector3 zeroVec = Vector3(0, 0, 0);
	D3DXMatrixLookAtLH( this, &zeroVec, &direction, &up );

	Matrix inverseMat;
	GetInverse( inverseMat );

	*this = inverseMat;
}

void Matrix::TransformCoord( Vector3 & result, const Vector3 & vector )
{
	D3DXVec3TransformCoord( &result, &vector, this );
}

void Matrix::TransformNormal( Vector3 & result, const Vector3 & normal )
{
	D3DXVec3TransformNormal( &result, &normal, this );
}

BOOL Matrix::GetInverse( Matrix & result )
{
	if( D3DXMatrixInverse( &result, NULL, this ) == NULL )
	{
		return FALSE;
	}

	return TRUE;
}

BOOL Matrix::GetInverseTransporse( Matrix & result )
{
	if( GetInverse( result ) )
	{
		result.GetTranspose( result );

		return TRUE;
	}

	return FALSE;
}

void Matrix::GetTranspose( Matrix & result )
{
	D3DXMatrixTranspose( &result, this );
}

void Matrix::SetTranslation( const Vector3 & pos )
{
	*reinterpret_cast<Vector3*>(m[3]) = pos;
}

Vector3 Matrix::GetTranslation()
{
	return *reinterpret_cast<Vector3*>(m[3]);
}

Vector3 Matrix::GetScale()
{
	return Vector3( m[0][0], m[1][1], m[2][2] );
}

float Matrix::GetYaw()
{
	return GetYawPitchRoll().x;
}

float Matrix::GetPitch()
{
	return GetYawPitchRoll().y;
}

float Matrix::GetRoll()
{
	return GetYawPitchRoll().z;
}

Vector3 Matrix::GetYawPitchRoll()
{
	float yaw, pitch, roll;

	if( m[2][1] < 1.f )
	{
		if( m[2][1] > -1.f )
		{
			yaw = atan2f(m[2][0],m[2][2]);
			pitch = asinf(-m[2][1]);
			roll = atan2f(m[0][1],m[1][1]); 
		}
		else
		{
			yaw = atan2f(m[1][0],m[0][0]);
			pitch = PI/2;
			roll = 0.f;
		}
	}
	else
	{
		yaw = atan2f(-m[1][0],m[0][0]);
		pitch = -PI/2;
		roll = 0.f;
	}

	return Vector3( yaw, pitch, roll );
}

void Matrix::PreMultiply( const Matrix & other )
{
	D3DXMatrixMultiply( this, &other, this );
}

void Matrix::PostMultiply( const Matrix & other )
{
	D3DXMatrixMultiply( this, this, &other );
}